#include "AP_Estimateur.h"

void AP_Estimateur::prediction(
		GPose3D pose_p,
		GPoint3D v_p,
		long int dtms,
		GPose3D& pose_s,
		GPoint3D& v_s) {
	float ax, ay;
	float cos_psi, sin_psi, tan_phi, cos_theta, tan_theta;
	float dt = dtms / 1000.0;
	cos_psi = cos(pose_p.yaw());
	sin_psi = sin(pose_p.yaw());
	tan_phi = tan(pose_p.roll());
	cos_theta = cos(pose_p.pitch());
	tan_theta = tan(pose_p.pitch());
	if (cos_theta != 0) {
		ax = cos_psi * tan_theta + sin_psi * (tan_phi / cos_theta);
		ay = sin_psi * tan_theta - cos_psi * (tan_phi / cos_theta);
		ax *= SIMU_MRPT ? 10 : 9.81;
		ay *= SIMU_MRPT ? 10 : 9.81;
	} else {
		ax = 0;
		ay = 0;
		// (c'est impossible)
	}
	pose_s.x(pose_p.m_x + dt * v_p.x + .5 * dt * dt * ax);
	pose_s.y(pose_p.m_y + dt * v_p.y + .5 * dt * dt * ay);
	v_s.x = v_p.x + dt * ax;
	v_s.y = v_p.y + dt * ay;
}

void AP_Estimateur::estimateur(
		GPose3D pose_m,
		GPoint3D v_m,
		GPose3D pose_p,
		GPoint3D v_p,
		long int dtms,
		float kx,
		float kv,
		GPose3D& pose_e_,
		GPoint3D& v_e_) {
	GPose3D pose_pred;
	GPoint3D v_pred;
	prediction(pose_p, v_p, dtms, pose_pred, v_pred);
	pose_e_.x(pose_pred.x() + kx * (pose_m.x() - pose_pred.x()));
	pose_e_.y(pose_pred.y() + kx * (pose_m.y() - pose_pred.y()));
	v_e_.x = v_pred.x + kv * (v_m.x - v_pred.x);
	v_e_.y = v_pred.y + kv * (v_m.y - v_pred.y);
}

void AP_Estimateur::estimateur(
		GPose3D pose_m,
		GPoint3D v_m,
		long int dtms,
		float kx,
		float kv) {
	GPose3D pose_p = pose_e;
	GPoint3D v_p = v_e;
	estimateur(pose_m, v_m, pose_p, v_p, dtms, kx, kv, pose_e, v_e);
	pose_e.setYawPitchRoll(pose_m.yaw(), pose_m.pitch(), pose_m.roll());
	pose_e.z(pose_m.z());
}

AP_Estimateur::AP_Estimateur() {
	pose_e = GPose3D(0, 0, 0, 0, 0, 0);
	v_e = GPoint3D(0.0, 0.0, 0.0);
	kx = 0.455855;
	kv = 0.54505;
}